#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>

class LidarDisplayNode
{
private:
    ros::NodeHandle nh_;
    ros::Subscriber lidar_sub_;
    pcl::visualization::CloudViewer viewer_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_;
    bool has_data_;

public:
    LidarDisplayNode() : viewer_("激光雷达点云"), has_data_(false)
    {
        cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
        
        lidar_sub_ = nh_.subscribe("/rslidar_points", 10, &LidarDisplayNode::lidarCallback, this);
        
        ROS_INFO("激光雷达点云显示节点已启动");
        ROS_INFO("订阅话题: /rslidar_points");
        ROS_INFO("PCL窗口标题: 激光雷达点云");
    }

    void lidarCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
    {
        try
        {
            // 转换ROS点云消息为PCL格式
            pcl::fromROSMsg(*msg, *cloud_);
            
            // 检查点云是否为空
            if (cloud_->empty())
            {
                ROS_WARN("接收到空的点云数据");
                return;
            }
            
            // 更新PCL可视化器
            viewer_.showCloud(cloud_);
            
            // 定期输出点云信息（每50次输出一次）
            static int count = 0;
            if (++count % 50 == 0)
            {
                ROS_INFO("点云数据: %zu 个点", cloud_->size());
            }
            
            has_data_ = true;
        }
        catch (const std::exception& e)
        {
            ROS_ERROR("点云处理异常: %s", e.what());
        }
    }

    void run()
    {
        ros::Rate rate(30); // 提高刷新率
        
        while (ros::ok() && !viewer_.wasStopped())
        {
            ros::spinOnce();
            rate.sleep();
        }
        
        ROS_INFO("激光雷达点云显示节点已停止");
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lidar_display_node");
    
    LidarDisplayNode node;
    node.run();
    
    return 0;
}